/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file telemPages.h

  \author Cory Dixon
*/

#include "telemPage.h"
#include "vcConfig.h"
#include "color.h"

#define BATT_CAPACITY 10.0    //[Ah]
#define BATT_SAFE_AH  2.0     //[Ah]
#define MAX_PW        1929.0  //[ms]
#define MIN_PW        1103.0  //[ms]
#define UPPER_CUTOFF  1929.0  //[ms]
#define LOWER_CUTOFF  1370.0  //[ms]

#define ETA_MEAS_WEIGHT  0.1

extern GuiStruct    guiStr;
extern FifoMsg_t  * fifo;

extern String piccoloGSCommIPStr;
extern String piccoloGSCommPortStr;

bool TelemPage::create_page(Client *client, GtkBuilder *builder)
{
	if( !client ) {
		cout << "TelemPage::create_page -- invalid client " << endl;
		return false;
	}
	if( page ) {
		cout << "TelemPage::create_page -- page has already been created" << endl;
		return true;
	}
	if( !builder ) {
		cout << "TelemPage::create_page -- invalid xml builder" << endl;
		return false;
	}

	timer_running = false;

	page_client = client;
	page        = GTK_WIDGET (gtk_builder_get_object(builder, "telem_page"));
	gps_lat     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_lat"));
	gps_lon     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_lon"));
	gps_heading = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_heading"));
	gps_height  = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_height"));
	gps_speed   = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_speed"));
	gps_pdop    = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_pdop"));
	gps_sat     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_sat"));
	current_wp  = GTK_WIDGET (gtk_builder_get_object(builder, "telem_current_wp"));
	goto_wp     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_goto_wp"));
	ad_tas      = GTK_WIDGET (gtk_builder_get_object(builder, "telem_ad_tas"));
	ad_alt      = GTK_WIDGET (gtk_builder_get_object(builder, "telem_ad_alt"));

	gps_agl     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_gps_agl"));

	rpm_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "rpm_stat"));
	alt_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "alt_stat"));
	ias_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "ias_stat"));
	att_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "att_stat"));
	sys_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "sys_stat"));
	gps_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "gps_stat"));
	link_stat     = GTK_WIDGET (gtk_builder_get_object(builder, "link_stat"));
	com_stat      = GTK_WIDGET (gtk_builder_get_object(builder, "com_stat"));
	term_stat     = GTK_WIDGET (gtk_builder_get_object(builder, "termination_stat"));
	engine_stat   = GTK_WIDGET (gtk_builder_get_object(builder, "engine_stat"));
	deadman_stat  = GTK_WIDGET (gtk_builder_get_object(builder, "deadman_stat"));
	main_v        = GTK_WIDGET (gtk_builder_get_object(builder, "telem_main_v"));
	main_ma       = GTK_WIDGET (gtk_builder_get_object(builder, "telem_main_ma"));
	servo_v       = GTK_WIDGET (gtk_builder_get_object(builder, "telem_servo_v"));
	servo_ma      = GTK_WIDGET (gtk_builder_get_object(builder, "telem_servo_ma"));
	flight_time   = GTK_WIDGET (gtk_builder_get_object(builder, "flight_time"));

	gs_back_ip       = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_ip"));
	gs_back_port     = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_port"));
	gs_back_serial   = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_serial"));
	gs_back_connect  = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_connect"));
	gchar *text;
	text = g_strdup_printf ("%s", piccoloGSCommIPStr.c_str());
	gtk_entry_set_text (GTK_ENTRY (gs_back_ip), text);
	g_free (text);
	text = g_strdup_printf ("%s", piccoloGSCommPortStr.c_str());
	gtk_entry_set_text (GTK_ENTRY (gs_back_port), text);
	g_free (text);

	gs_back_telem  = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_telem"));
	gs_back_fp  = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_fp"));
	gs_back_wind  = GTK_WIDGET (gtk_builder_get_object(builder, "gs_back_wind"));

	rssi = GTK_WIDGET (gtk_builder_get_object(builder, "telem_rssi"));
	throttle = GTK_WIDGET (gtk_builder_get_object(builder, "telem_throttle"));
	loop_ias_telem = GTK_WIDGET (gtk_builder_get_object(builder, "telem_loop_ias_telem"));
	loop_ias = GTK_WIDGET (gtk_builder_get_object(builder, "telem_loop_ias"));
	goto_ias = GTK_WIDGET (gtk_builder_get_object(builder, "telem_goto_ias"));
	loop_ias_send = GTK_WIDGET (gtk_builder_get_object(builder, "loop_ias_send"));
	loop_alt_telem = GTK_WIDGET (gtk_builder_get_object(builder, "telem_loop_alt_telem"));
	loop_alt = GTK_WIDGET (gtk_builder_get_object(builder, "telem_loop_alt"));
	goto_alt = GTK_WIDGET (gtk_builder_get_object(builder, "telem_goto_alt"));
	loop_alt_send = GTK_WIDGET (gtk_builder_get_object(builder, "loop_alt_send"));

	wp_off = GTK_WIDGET (gtk_builder_get_object(builder, "wp_off"));
	wp_on = GTK_WIDGET (gtk_builder_get_object(builder, "wp_on"));
	ias_off = GTK_WIDGET (gtk_builder_get_object(builder, "ias_off"));
	ias_on = GTK_WIDGET (gtk_builder_get_object(builder, "ias_on"));
	ias_auto = GTK_WIDGET (gtk_builder_get_object(builder, "ias_auto"));
	alt_off = GTK_WIDGET (gtk_builder_get_object(builder, "alt_off"));
	alt_on = GTK_WIDGET (gtk_builder_get_object(builder, "alt_on"));
	alt_auto = GTK_WIDGET (gtk_builder_get_object(builder, "alt_auto"));

	batt_ah = GTK_WIDGET (gtk_builder_get_object(builder, "telem_batt_ah"));
	batt_amps = GTK_WIDGET (gtk_builder_get_object(builder, "telem_batt_amps"));
	batt_ah_reset = GTK_WIDGET (gtk_builder_get_object(builder, "telem_batt_ah_reset"));
	reset_batt_stats();

	eta_estimate = GTK_WIDGET (gtk_builder_get_object(builder, "telem_eta_estimate"));
	headwind_field = GTK_WIDGET (gtk_builder_get_object(builder, "telem_headwind"));
	reset_eta();

	return_eta_field = GTK_WIDGET (gtk_builder_get_object(builder, "telem_return_eta"));

	// get buttons, and add signal handlers
	wp_send     = GTK_WIDGET (gtk_builder_get_object(builder, "telem_wp_send"));
	timer_ss    = GTK_WIDGET (gtk_builder_get_object(builder, "timer_ss_toggle"));
	timer_reset = GTK_WIDGET (gtk_builder_get_object(builder, "timer_reset_button"));

	// make sure we got all of our widgets
	if( !( page && gps_lat && gps_lon  && gps_heading && gps_height &&
	    gps_speed && gps_pdop && gps_sat && current_wp && goto_wp &&
			ad_tas && ad_alt && gps_agl && wp_send && 
			rpm_stat && alt_stat && ias_stat && att_stat && sys_stat && gps_stat &&
			link_stat && com_stat && term_stat && engine_stat && deadman_stat &&
			main_v && main_ma && servo_v && servo_ma && timer_ss &&
		 	flight_time && timer_reset &&
			gs_back_ip && gs_back_port && gs_back_serial && gs_back_connect &&
			rssi && throttle &&
			loop_ias_telem && loop_ias && goto_ias && loop_ias_send &&
			loop_alt_telem && loop_alt && goto_alt && loop_alt_send &&
			wp_off && wp_on &&
			ias_off && ias_on && ias_auto &&
			alt_off && alt_on && alt_auto &&
			batt_ah && batt_amps && batt_ah_reset&&
			eta_estimate && headwind_field
		) ) {

		page = NULL;
		return false;
	}

	g_signal_connect(G_OBJECT(wp_send), "clicked", G_CALLBACK(TelemPage::wp_send_clicked), this);
	g_signal_connect(G_OBJECT(goto_wp), "key-press-event", G_CALLBACK(TelemPage::goto_wp_key_press), NULL);
	g_signal_connect(G_OBJECT(timer_ss), "toggled", G_CALLBACK(TelemPage::timer_ss_toggle), this);
	g_signal_connect(G_OBJECT(timer_reset), "clicked", G_CALLBACK(TelemPage::timer_reset_clicked), this);
	g_signal_connect(G_OBJECT(gs_back_connect), "clicked", G_CALLBACK(TelemPage::gs_back_connect_clicked), this);

	g_signal_connect(G_OBJECT(loop_ias_send), "clicked", G_CALLBACK(TelemPage::loop_ias_send_clicked), this);
	g_signal_connect(G_OBJECT(loop_alt_send), "clicked", G_CALLBACK(TelemPage::loop_alt_send_clicked), this);

	g_signal_connect(G_OBJECT(batt_ah_reset), "clicked", G_CALLBACK(TelemPage::batt_ah_reset_clicked), this);

	loop_init = false;
	return true;
}

void TelemPage::update()
{
	if( !page_client ) {
		cout << "TelemPage::update() - invalid page_client" << endl;
		return;
	}
	//-- See if the page has been initialized
	if( page == NULL ) {
		cout << "TelemPage::update -- page has not been created" << endl;
		return;
	}

	// don't do work if we don't need too
	if( !GTK_WIDGET_VISIBLE( page ) ) {
		return;
	}

	gchar *text;

	// address
	text = g_strdup_printf ("%d", page_client->telemetry.address);
	gtk_entry_set_text (GTK_ENTRY (gs_back_serial), text);
	g_free (text);

	// lat
	text = g_strdup_printf ("%.6f", page_client->telemetry.gps.pos.lat);
	gtk_entry_set_text (GTK_ENTRY (gps_lat), text);
	g_free (text);
		
	// lon
	text = g_strdup_printf ("%.6f", page_client->telemetry.gps.pos.lon);
	gtk_entry_set_text (GTK_ENTRY (gps_lon), text);
	g_free (text);

	// heading
	text = g_strdup_printf ("%.1f", page_client->telemetry.gps.track);
	gtk_entry_set_text (GTK_ENTRY (gps_heading), text);
	g_free (text);
	
	// height
	text = g_strdup_printf ("%.1f", page_client->telemetry.gps.pos.alt);
	gtk_entry_set_text (GTK_ENTRY (gps_height), text);
	g_free (text);

	//approximate AGL
	Client *me = guiStr.shm_ptr->clients.getClient(0);
	if(me->state.gps)
		text = g_strdup_printf ("%.1f", page_client->telemetry.gps.pos.alt - me->telemetry.gps.pos.alt);
	else
		text = g_strdup_printf ("NaN");
	gtk_entry_set_text (GTK_ENTRY (gps_agl), text);
	g_free (text);
	
	// speed
	text = g_strdup_printf ("%.1f", page_client->telemetry.gps.sog);
	gtk_entry_set_text (GTK_ENTRY (gps_speed), text);
	g_free (text);
	
	// pdop
	text = g_strdup_printf ("%.1f", page_client->telemetry.gps.accuracy.pdop);
	gtk_entry_set_text (GTK_ENTRY (gps_pdop), text);
	g_free (text);
	
	// sats
	text = g_strdup_printf ("%i", page_client->telemetry.gps.accuracy.sats);
	gtk_entry_set_text (GTK_ENTRY (gps_sat), text);
	g_free (text);
		
	// TAS
	text = g_strdup_printf ("%.1f", page_client->telemetry.airdata.tas);
	gtk_entry_set_text (GTK_ENTRY (ad_tas), text);
	g_free (text);

	// alt
	text = g_strdup_printf ("%.1f", page_client->telemetry.airdata.alt);
	gtk_entry_set_text (GTK_ENTRY (ad_alt), text);
	g_free (text);

	// loop target IAS
	text = g_strdup_printf ("%.1f", page_client->telemetry.airdata.ias);
	gtk_entry_set_text (GTK_ENTRY (loop_ias_telem), text);
	g_free (text);

	// loop target IAS
	text = g_strdup_printf ("%.1f", page_client->telemetry.navigation.targetIAS);
	gtk_entry_set_text (GTK_ENTRY (loop_ias), text);
	g_free (text);
	
	// loop target alt
	text = g_strdup_printf ("%.1f", page_client->telemetry.airdata.alt);
	gtk_entry_set_text (GTK_ENTRY (loop_alt_telem), text);
	g_free (text);

	if(!loop_init) {
		if((page_client->telemetry.navigation.targetAltCmd) == 0)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_off),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_off),false);

		if((page_client->telemetry.navigation.targetAltCmd) == 1)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_on),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_on),false);

		if((page_client->telemetry.navigation.targetAltCmd) == 2)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_auto),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (alt_auto),false);

		if((page_client->telemetry.navigation.targetIASCmd) == 0)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_off),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_off),false);

		if((page_client->telemetry.navigation.targetIASCmd) == 1)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_on),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_on),false);

		if((page_client->telemetry.navigation.targetIASCmd) == 2)
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_auto),true);
		else
			gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON (ias_auto),false);
		if(page_client->state.gps)
			loop_init = true;
	}


	// loop target alt
	text = g_strdup_printf ("%.1f", page_client->telemetry.navigation.targetAlt);
	gtk_entry_set_text (GTK_ENTRY (loop_alt), text);
	g_free (text);
	
	// current waypoint
	text = g_strdup_printf ("%i", page_client->telemetry.navigation.waypt);
	gtk_entry_set_text (GTK_ENTRY (current_wp), text);
	g_free (text);
	
	//if( page_client->state.gps ) {
		// altitude status
		if( page_client->telemetry.limits.altitudeLow || page_client->telemetry.limits.altitudeHigh )
			set_bad_state(alt_stat);
		else
			set_good_state(alt_stat);

		// ias status
		if( page_client->telemetry.limits.IASLow || page_client->telemetry.limits.IASHigh )
			set_bad_state(ias_stat);
		else
			set_good_state(ias_stat);

		// attitude status
		if( page_client->telemetry.limits.rollHigh || page_client->telemetry.limits.pitchHigh )
			set_bad_state(att_stat);
		else
			set_good_state(att_stat);

		// rpm status
		if( page_client->telemetry.limits.RPMLow || page_client->telemetry.limits.RPMHigh )
			set_bad_state(rpm_stat);
		else
			set_good_state(rpm_stat);

		// gps status
		if( page_client->telemetry.failure.gpsTimeout )
			set_bad_state(gps_stat);
		else
			set_good_state(gps_stat);

		// com status
		if( page_client->telemetry.failure.commTimeout )
			set_bad_state(com_stat);
		else
			set_good_state(com_stat);

		// termination status
		if( page_client->telemetry.failure.aeroTermination || page_client->telemetry.failure.flightTermination )
			set_bad_state(term_stat);
		else
			set_good_state(term_stat);

		// deadman status
		if( page_client->telemetry.failure.deadman )
			set_good_state(deadman_stat);
		else
			set_bad_state(deadman_stat);

		// termination status
		if( page_client->telemetry.failure.manual_engineKill || page_client->telemetry.failure.auto_engineKill || page_client->telemetry.failure.operator_engineKill )
			set_bad_state(engine_stat);
		else
			set_good_state(engine_stat);

		// system status (as defined by piccolo docs, note values are in mV)
		if( page_client->telemetry.system.twelveV >= 15000 || page_client->telemetry.system.twelveV <= 11000 
		 || page_client->telemetry.system.boardT >= 80 || page_client->telemetry.system.boardT <= 0 )
			set_bad_state(sys_stat);
		else
			set_good_state(sys_stat);

		// link status, we don't have a measure of ack ratio, so we can only
		// downlink the airplanes RSSI. So set error based on a conservative value of
		// -93 dBm. Note piccolo has this at -101 or 85% packet. 
		if( page_client->telemetry.system.RSSI <= -93 ) 
			set_bad_state(link_stat);
		else
			set_good_state(link_stat);
		

		// set system status values
		text = g_strdup_printf ("%.2f", page_client->telemetry.system.twelveV / 1000.0);
		gtk_entry_set_text (GTK_ENTRY (main_v), text);
		g_free (text);

		text = g_strdup_printf ("%i", page_client->telemetry.system.twelveA);
		gtk_entry_set_text (GTK_ENTRY (main_ma), text);
		g_free (text);

		text = g_strdup_printf ("%.2f", page_client->telemetry.system.fiveV / 1000.0);
		gtk_entry_set_text (GTK_ENTRY (servo_v), text);
		g_free (text);

		text = g_strdup_printf ("%i", page_client->telemetry.system.fiveA);
		gtk_entry_set_text (GTK_ENTRY (servo_ma), text);
		g_free (text);

		text = g_strdup_printf ("%i", page_client->telemetry.system.RSSI);
		gtk_entry_set_text (GTK_ENTRY (rssi), text);
		g_free (text);

		text = g_strdup_printf ("%i", (int)(page_client->telemetry.servos.channels[2]/655.35));
		gtk_entry_set_text (GTK_ENTRY (throttle), text);
		g_free (text);
	//}

	// flight timer
	if(((page_client->telemetry.servos.channels[2]/655.35) > 80) &&
			(page_client->telemetry.airdata.ias > 15.0)) {
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(timer_ss) ,true );
	}

	text = NULL;
	if(flight_timer_ts == 0 ) {
		text = g_strdup_printf (" --- ");
	} else { 
		if(timer_running) {
			TimeStamp now;
			now.stamp();
			timer_val = (now - flight_timer_ts) / SEC2MICRO;
			if(timer_val > 60) {
				timer_minutes += (int)(timer_val/60);
				flight_timer_ts += (long int)((int)(timer_val/60) * 60 * SEC2MICRO);
				timer_val -= (int)(timer_val/60) * 60;
			}
		}
		//int mm      = (int)(timer_val / 60);
		int mm      = timer_minutes;
		//float ss    = (timer_val - mm * 60);
		float ss    = timer_val;
		text = g_strdup_printf ("%i:%04.1f", mm,ss);
	}
	if( text ) {
		gtk_entry_set_text (GTK_ENTRY (flight_time), text);
		g_free (text);
	}
	
	if( page_client->state.gs_telem)
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_telem),1 ); 
	else
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_telem),0 ); 

	if( page_client->state.gs_wind)
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_wind),1 ); 
	else
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_wind),0 ); 

	if( page_client->state.gs_fp)
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_fp),1 ); 
	else
		gtk_toggle_button_set_active( GTK_TOGGLE_BUTTON(gs_back_fp),0 ); 

	bool engine_enabled = page_client->telemetry.failure.deadman && gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(timer_ss));
	estimate_ah(
			(page_client->telemetry.servos.channels[2]/655.35),
			engine_enabled);

	text = g_strdup_printf ("%.1f", battery_amp_hours*1000.0);
	gtk_entry_set_text (GTK_ENTRY (batt_ah), text);
	g_free (text);

	text = g_strdup_printf ("%.1f", battery_amps);
	gtk_entry_set_text (GTK_ENTRY (batt_amps), text);
	g_free (text);

	estimate_eta(
			page_client->telemetry.airdata.ias,
			GPSBearing(me->telemetry.gps.pos, page_client->telemetry.gps.pos),
		  GPSDist(me->telemetry.gps.pos, page_client->telemetry.gps.pos),
			-(page_client->wind.south),
			-(page_client->wind.west));

	if(eta <= 0.0)
		text = g_strdup_printf ("NA");
	else
		text = g_strdup_printf ("%d:%02d", int(eta/60.0), int(fmod(eta,60.0)));
	gtk_entry_set_text (GTK_ENTRY (eta_estimate), text);
	g_free (text);
	
	estimate_turn_eta();

	text = g_strdup_printf ("%d:%02d", int(return_eta/60.0), int(fabs(fmod(return_eta,60.0))));
	gtk_entry_set_text (GTK_ENTRY (return_eta_field), text);
	g_free (text);

	text = g_strdup_printf ("%d", int(headwind));
	gtk_entry_set_text (GTK_ENTRY (headwind_field), text);
	g_free (text);

	// redraw page
	gtk_widget_queue_draw(page);	
}

//-----------------------------------------------------------
// button handlers -- we pass a WindowStruct pointer
void TelemPage::wp_send_clicked(GtkButton *button, gpointer data)
{
	//cout << "TelemPage::wp_send_clicked" << endl;
	
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	// get the goto waypoint value
	const gchar *valStr = gtk_entry_get_text( GTK_ENTRY(telem->goto_wp) );

	int length = strlen(valStr);
	if( length > 0 ) {
		int val = atoi(valStr);
		if( val >= 0 && val <= 255) {
			GotoWP_t *wp = (GotoWP_t *)CMD_DATAPTR(fifo->data);
			wp->ind = val;

			Client *me = guiStr.shm_ptr->clients.getClient(0);
			fifo->src.s_addr = guiStr.shm_ptr->clients.getAddr(me, telem->page_client).s_addr;
			fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(telem->page_client).s_addr;

			CMD_CMD(fifo->data)  = CMD_GOTO_WP;
			CMD_NUM(fifo->data)  = sizeof(GotoWP_t);
			CMD_CSUM(fifo->data) = 0;
			CMD_CSUM(fifo->data) = C_calcCheckSum((uint8_t*)fifo->data);

			fifo->size = CMD_SIZE(fifo->data);
			guiStr.vcgui->writeFifo(ThreadedPipe::INBOARD_TX);

			printf("<-- Sent %s CMD_GOTO_WP=%i -->\n", telem->page_client->name.c_str(), val);	

			telem->set_sent_state(GTK_WIDGET(button));

		} else {
			gtk_entry_set_text (GTK_ENTRY (telem->goto_wp), "");
			cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
		}
	}

	return;
}

void TelemPage::loop_ias_send_clicked(GtkButton *button, gpointer data)
{
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	// get the goto waypoint value
	const gchar *valStr = gtk_entry_get_text( GTK_ENTRY(telem->goto_ias) );
	bool is_off  = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->ias_off)); 
	bool is_on   = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->ias_on)); 
	bool is_auto = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->ias_auto)); 

	int length = strlen(valStr);
	if( length > 0 ) {
		int val = atoi(valStr);
		if( val >= 0 ) {
			GotoIAS_t *ias = (GotoIAS_t *)CMD_DATAPTR(fifo->data);
			ias->value = val;
			if(is_off)  ias->control = 0;
			if(is_on)   ias->control = 1;
			if(is_auto) ias->control = 2;

			Client *me = guiStr.shm_ptr->clients.getClient(0);
			fifo->src.s_addr = guiStr.shm_ptr->clients.getAddr(me, telem->page_client).s_addr;
			fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(telem->page_client).s_addr;

			CMD_CMD(fifo->data)  = CMD_IAS_LOOP;
			CMD_NUM(fifo->data)  = sizeof(GotoIAS_t);
			CMD_CSUM(fifo->data) = 0;
			CMD_CSUM(fifo->data) = C_calcCheckSum((uint8_t*)fifo->data);

			fifo->size = CMD_SIZE(fifo->data);
			guiStr.vcgui->writeFifo(ThreadedPipe::INBOARD_TX);

			printf("<-- Sent %s CMD_GOTO_IAS=%i CONTROL=%i -->\n", telem->page_client->name.c_str(), val, ias->control);	

			telem->set_sent_state(GTK_WIDGET(button));

		} else {
			cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
		}
	}
	return;
}

void TelemPage::loop_alt_send_clicked(GtkButton *button, gpointer data)
{
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	// get the goto waypoint value
	const gchar *valStr = gtk_entry_get_text( GTK_ENTRY(telem->goto_alt) );
	bool is_off  = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->alt_off)); 
	bool is_on   = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->alt_on)); 
	bool is_auto = gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(telem->alt_auto)); 

	int length = strlen(valStr);
	if( length > 0 ) {
		int val = atoi(valStr);
		if( val >= 0 ) {
			GotoAlt_t *alt = (GotoAlt_t *)CMD_DATAPTR(fifo->data);
			alt->value = val;
			if(is_off)  alt->control = 0;
			if(is_on)   alt->control = 1;
			if(is_auto) alt->control = 2;

			Client *me = guiStr.shm_ptr->clients.getClient(0);
			fifo->src.s_addr = guiStr.shm_ptr->clients.getAddr(me, telem->page_client).s_addr;
			fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(telem->page_client).s_addr;

			CMD_CMD(fifo->data)  = CMD_ALT_LOOP;
			CMD_NUM(fifo->data)  = sizeof(GotoAlt_t);
			CMD_CSUM(fifo->data) = 0;
			CMD_CSUM(fifo->data) = C_calcCheckSum((uint8_t*)fifo->data);

			fifo->size = CMD_SIZE(fifo->data);
			guiStr.vcgui->writeFifo(ThreadedPipe::INBOARD_TX);

			printf("<-- Sent %s CMD_GOTO_ALT=%i CONTROL=%i -->\n", telem->page_client->name.c_str(), val, alt->control);	

			telem->set_sent_state(GTK_WIDGET(button));

		} else {
			cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
		}
	}
	return;
}

void TelemPage::batt_ah_reset_clicked(GtkButton *widget, gpointer data) {
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	telem->reset_batt_stats();
}

//-----------------------------------------------------------
// button handlers -- we pass a WindowStruct pointer
void TelemPage::timer_ss_toggle(GtkButton *button, gpointer data)
{
	//cout << "TelemPage::timer_ss_toggle" << endl;
	
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	if( gtk_toggle_button_get_active( GTK_TOGGLE_BUTTON(button) ) ) {
		gtk_button_set_label(button,"Stop");
		telem->flight_timer_ts.stamp();
		if( telem->flight_timer_ts != 0 ) {
			telem->flight_timer_ts.stamp();
			telem->flight_timer_ts -= (long)(telem->timer_val * SEC2MICRO);
		}
		telem->timer_running = true;
	} else {
		//telem->flight_timer_ts = 0;
		gtk_button_set_label(button,"Start");
		telem->timer_running = false;
	}

	return;
}

void TelemPage::timer_reset_clicked(GtkButton *button, gpointer data)
{
	//cout << "TelemPage::timer_reset_clicked" << endl;
	
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	telem->flight_timer_ts.stamp();
	telem->timer_val = 0;
	telem->timer_minutes = 0;

	return;
}

void TelemPage::gs_back_connect_clicked(GtkButton *button, gpointer data) 
{
	TelemPage *telem = (TelemPage *)data;
	if( !telem )
		return;

	//telem->set_gs_back();
	
	const gchar *ipStr = gtk_entry_get_text( GTK_ENTRY(telem->gs_back_ip) );
	const gchar *portStr = gtk_entry_get_text( GTK_ENTRY(telem->gs_back_port) );

	int length = strlen(ipStr);
	if( length > 0 ) {
		int length = strlen(portStr);
		if( length > 0 ) {

			telem->set_sent_state(GTK_WIDGET(button));
			telem->set_gs_back();
			telem->connect_to_gs(ipStr,portStr);

		} else {
			cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
		}
	} else {
		cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
	}

	return;
}

void TelemPage::connect_to_gs(const gchar * ipStr, const gchar * portStr) {
	CommStruct_t *comm = (CommStruct_t *)CMD_DATAPTR(fifo->data);
	(comm->ip).s_addr = (inet_addr(ipStr));
	comm->port = atoi(portStr);

	if(page_client->state.gs_backup) {
		comm->connect = true;
	} else {
		comm->connect = false;
	}

	Client *me = guiStr.shm_ptr->clients.getClient(0);

	fifo->src.s_addr = guiStr.shm_ptr->clients.getAddr(me).s_addr;
	//fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(me, page_client).s_addr;
	fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(me).s_addr;

	CMD_CMD(fifo->data)  = CMD_CONNECT;
	CMD_NUM(fifo->data)  = sizeof(CommStruct_t);
	CMD_CSUM(fifo->data) = 0;
	CMD_CSUM(fifo->data) = C_calcCheckSum((uint8_t*)fifo->data);

	fifo->size = CMD_SIZE(fifo->data);
	guiStr.vcgui->writeFifo(ThreadedPipe::INBOARD_TX);
}

void TelemPage::set_gs_back(void) {
	if(page_client->state.gs_backup) {
		page_client->state.gs_backup = 0;
	} else {
		page_client->state.gs_backup = 1;
	}
}

gboolean TelemPage::goto_wp_key_press(GtkWidget *widget, GdkEventKey *event, gpointer user_data)
{
   gint k;
   k = event->keyval;

   // If not a valid WP number, don't do anything
   // and fully catch signal
   if( (k < '0' || k > '9') && (k < 65000) ) {
		printf("WARNING: dumb user at console - waypoints are defined by numbers\n");
      return TRUE;
   }

   // Let the number pass through
   return FALSE;
}

// use this function to indicate a command or ACK has been received
void TelemPage::setCMD_RX(uint8_t cmd_id, int ack_nack)
{
	//cout << "TelemPage::setCMD_RX - [" << (int)cmd_id << "]  ackNack=" << ack_nack << endl;
	
	switch( cmd_id ) {
		case CMD_GOTO_WP:
			set_rx_state(wp_send, ack_nack);
			break;
		case CMD_ALT_LOOP:
			set_rx_state(loop_alt_send, ack_nack);
			break;
		case CMD_IAS_LOOP:
			set_rx_state(loop_ias_send, ack_nack);
			break;
		case CMD_CONNECT:
			if(!ack_nack) {
				set_rx_state(gs_back_connect, ack_nack);
				page_client->state.gs_backup = 0;
			} else {
				if(page_client->state.gs_backup)
					set_good_state(gs_back_connect);
				else {
					set_rx_state(gs_back_connect, 1);
				}
			}
			break;

		default:
			break;
	}

	// update page
	if( page )
		gtk_widget_queue_draw(page);
}

void TelemPage::estimate_ah(float throttle, bool engine_enabled) {

	if(!engine_enabled) {
		battery_amps = 0.0;
		last_ah_estimate.stamp();
		return;
	}

	TimeStamp now;
	now.stamp();
	float delta_t = (now - last_ah_estimate) / SEC2MICRO;
	last_ah_estimate.stamp();

	float used_ah = 0.0;
	float amp_draw = 0.0;
	float pulse_width = throttle_lookup (throttle);

	if(pulse_width < LOWER_CUTOFF) used_ah = 0.0;
	else {
		if(pulse_width > UPPER_CUTOFF)
			pulse_width = UPPER_CUTOFF;
		float x = pulse_width / 1000.0;
		amp_draw = 113.3577*pow(x,2) -268.6539*x+  155.4587; // [A]
		used_ah = (delta_t / 3600.0) * amp_draw;
	}

	battery_amps = amp_draw;
	battery_amp_hours -= used_ah;

	amp_samples ++;
	average_amps = (amp_draw + (amp_samples-1)*average_amps)/amp_samples;
}

float TelemPage::throttle_lookup(float throttle) {
	float diff_pw = MAX_PW - MIN_PW;

	float throttle_pw = (throttle/100.0) * diff_pw + MIN_PW;
	return throttle_pw;
}

void TelemPage::reset_batt_stats() {
	battery_amp_hours = (float)BATT_CAPACITY;
	battery_amps = 0.0;

	average_amps = 0.0;
	amp_samples = 0;

	last_ah_estimate.stamp();
}

void TelemPage::reset_eta() {
	ias_avg = 0;
	wind_n_avg = 0;
	wind_e_avg = 0;
	eta = 0;
	return_eta = 0;
}
void TelemPage::estimate_eta(float ias, float bearing, float distance, float wind_n, float wind_e) {

	if(bearing < 0) bearing = 360.0 + bearing;

  ias_avg = ias_avg * (1.0-ETA_MEAS_WEIGHT) + ias * ETA_MEAS_WEIGHT;
  wind_n_avg = wind_n_avg * (1.0-ETA_MEAS_WEIGHT) + wind_n * ETA_MEAS_WEIGHT;
  wind_e_avg = wind_e_avg * (1.0-ETA_MEAS_WEIGHT) + wind_e * ETA_MEAS_WEIGHT;

	float wind_n_fraction = wind_n_avg * cos(bearing*M_PI/180.0);
	float wind_e_fraction = wind_e_avg * cos((bearing-90.0)*M_PI/180.0);

	headwind = -(wind_n_fraction + wind_e_fraction);
	float speed = ias - headwind;
	if (speed <= 0.0) eta = -1.0;
	else eta = eta * (1.0 - ETA_MEAS_WEIGHT) + distance/speed * ETA_MEAS_WEIGHT;

	/*cout << "bearing: " << bearing << endl;
	cout << "distance: " << distance << endl;
	cout << "east: " << -1* page_client->wind.west << endl;
	cout << "north: " << -1* page_client->wind.south << endl;
	cout << "east frac: " << wind_e_fraction << endl;
	cout << "north frac: " << wind_n_fraction << endl;
	cout << "headwind: " << headwind << endl;
	cout << "speed: " << speed << endl;
	cout << endl;*/
}

void TelemPage::estimate_turn_eta() {
	if(average_amps <= 0) return_eta = 0.0;
	else {
		float time_remaining = (battery_amp_hours - BATT_SAFE_AH) / average_amps * 3600;
		return_eta = time_remaining - eta;
	}
}

TelemPage::~TelemPage() {
	TimeStamp now; now.stamp();
	timer_val = (now - flight_timer_ts) / SEC2MICRO;
	if(timer_val > 60) {
		timer_minutes += (int)(timer_val/60);
		flight_timer_ts += (long int)((int)(timer_val/60) * 60 * SEC2MICRO);
		timer_val -= (int)(timer_val/60) * 60;
	}
	int mm = timer_minutes;
	int ss = (int)timer_val;

  cout << page_client->name << " flight time " << mm << ":"<< ss << " " << battery_amp_hours * 1000.0 << " mAH remaining" << endl;
}
